#ifndef RSIM_JOINTUTILS_H
#define RSIM_JOINTUTILS_H

/* -------------------------------------------------------------------------- *
 * File: JointUtils.h                                                         *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

namespace RSIM{
/** 
The functions of this class are generated using the command ccode in Matlab 
Refer http://www.mathworks.com/help/toolbox/symbolic/ccode.html for more details
*/
class JointUtils{
        public:
                JointUtils(){}
                
                /** [cFk]_{6x1} = [Fk3]_{6x1} - [Ik3]_{6x6}*[Akt]_{6x1} */
                void calc_cFk(const double *Fk3, const double *Ik3, const double *Akt, double *cFk) const;
                
                /** [mF]_{6x1} = [I]_{6x6}*[A]_{6x1}-  [Fk3]_{6x1}  */
                void calc_m_cFk(const double *F, const double *I, const double *A, double *mF) const;
                
                /** skbeta * I * transpose(skbeta), skbeta = [eye(3,3), beta_x; zeros(3,3), eye(3,3)] */
                void calc_skbeta_x_Ik3_x_tskbeta(const double *Ik3, const double *beta, double *sk_I_tsk)const;
                
                /** skbeta * cFk */
                void calc_skbeta_x_cFk(const double *cFk, const double *beta, double *skbeta_x_cFk)const;

                // The functions Triang_x_cFk and Triang_x_cFk have a common structure which is as follows.
                // Let H be the spatial vector representing all possible degrees of freedom of this body expressed
                // in F frame of *this* body. e.g, if a body rotates just about its y axis of frame F with respect
                // to parent, then H = [0 1 0 0 0 0]' (using matlab syntax)
                // If I and F denote the ABI and bias force for this body, let Z =(H')*I*H.
                // Then T = (eye(6,6) - I*H*H'/Z). The functions below directly give the values of T*I and 
                // T*cFk (cFk = F-I*akt) for specific values of H (only 6 different values of H are possible anyway. 
                // So the functions are hardcoded)
                
                // calc_UDot functions compute -H'*(I*A-F)/(H'*I*H) for 6 different values of H
        
                // Rotation about x axis, H = [1 0 0 0 0 0]'
                void calc_Triang_x_Ik3_RotX(const double *Ik3, double *Triang_x_Ik3)const;
                void calc_Triang_x_cFk_RotX(const double *Ik3, const double *cFk, double *Triang_x_cFk)const;
                void calc_UDot_RotX(const double *Ik3, const double *Ak1, const double *cFk, double *UDot)const;
                
                // Rotation about y axis, H = [0 1 0 0 0 0]'
                void calc_Triang_x_Ik3_RotY(const double *Ik3, double *Triang_x_Ik3)const;
                void calc_Triang_x_cFk_RotY(const double *Ik3, const double *cFk, double *Triang_x_cFk)const;
                void calc_UDot_RotY(const double *Ik3, const double *Ak1, const double *cFk, double *UDot)const;
                
                // Rotation about z axis, H = [0 0 1 0 0 0]'
                void calc_Triang_x_Ik3_RotZ(const double *Ik3, double *Triang_x_Ik3)const;
                void calc_Triang_x_cFk_RotZ(const double *Ik3, const double *cFk, double *Triang_x_cFk)const;
                void calc_UDot_RotZ(const double *Ik3, const double *Ak1, const double *cFk, double *UDot)const;
                
                // Translation along x axis, H = [0 0 0 1 0 0]'
                void calc_Triang_x_Ik3_TransX(const double *I, double *triang_x_ik3)const;
                void calc_Triang_x_cFk_TransX(const double *I, const double *F, double *triang_x_cfk)const;
                void calc_UDot_TransX(const double *Ik3, const double *Ak1, const double *cFk, double *UDot)const;
                
                // Translation along y axis, H = [0 0 0 0 1 0]'
                void calc_Triang_x_Ik3_TransY(const double *I, double *triang_x_ik3)const;
                void calc_Triang_x_cFk_TransY(const double *I, const double *F, double *triang_x_cfk)const;
                void calc_UDot_TransY(const double *Ik3, const double *Ak1, const double *cFk, double *UDot)const;
                
                // Translation along x axis, H = [0 0 0 0 0 1]'
                void calc_Triang_x_Ik3_TransZ(const double *I, double *triang_x_ik3)const;
                void calc_Triang_x_cFk_TransZ(const double *I, const double *F, double *triang_x_cfk)const;
                void calc_UDot_TransZ(const double *Ik3, const double *Ak1, const double *cFk, double *UDot)const;
                
}; // class JointUtils

inline void JointUtils::calc_UDot_RotX(const double *I, const double *A, const double *F, double *UDot)const{
        UDot[0] = -(A[0]*I[0] - F[0] + A[1]*I[1] + A[2]*I[2] + A[3]*I[3] + A[4]*I[4] + A[5]*I[5])/I[0];
}

inline void JointUtils::calc_UDot_RotY(const double *I, const double *A, const double *F, double *UDot)const{
        UDot[0] = -(A[0]*I[6] - F[1] + A[1]*I[7] + A[2]*I[8] + A[3]*I[9] + A[4]*I[10] + A[5]*I[11])/I[7];
}

inline void JointUtils::calc_UDot_RotZ(const double *I, const double *A, const double *F, double *UDot)const{
        UDot[0] = -(A[0]*I[12] - F[2] + A[1]*I[13] + A[2]*I[14] + A[3]*I[15] + A[4]*I[16] + A[5]*I[17])/I[14];
}

inline void JointUtils::calc_UDot_TransX(const double *I, const double *A, const double *F, double *UDot)const{
        UDot[0] = -(A[0]*I[18] - F[3] + A[1]*I[19] + A[2]*I[20] + A[3]*I[21] + A[4]*I[22] + A[5]*I[23])/I[21];
}

inline void JointUtils::calc_UDot_TransY(const double *I, const double *A, const double *F, double *UDot)const{
        UDot[0] = -(A[0]*I[24] - F[4] + A[1]*I[25] + A[2]*I[26] + A[3]*I[27] + A[4]*I[28] + A[5]*I[29])/I[28];
}

inline void JointUtils::calc_UDot_TransZ(const double *I, const double *A, const double *F, double *UDot)const{
        UDot[0] = -(A[0]*I[30] - F[5] + A[1]*I[31] + A[2]*I[32] + A[3]*I[33] + A[4]*I[34] + A[5]*I[35])/I[35];
}

} // namespace RSIM

#endif